/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file gpsParser.cxx
  \brief class implemenation for gpsParser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <iostream>
#include <math.h>

#include "gpsParser.h"
#include "color.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---External Global Variables---->*/
extern bool verbose;

/*<-Global Variables-->*/

GPSParser::GPSParser(String name, int a_port) : ThreadedPipe(name)
{
	cout << GREEN << "---------->>>>>       "YELLOW"GPS"GREEN"       <<<<<----------" << ATTROFF << endl;
	GPSInit(gps);
	buf_ptr = 0;
	port = a_port;
}

bool GPSParser::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	transmit_data = false;
	return true;
}

void GPSParser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("GPSParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "GPSParser::update() - select interrupted" << endl;
		} else cout << getName() << "::GPSParser::update() - select timed out" << endl;
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			//cout << "readFifo n=" << n << endl;
			if(n>0) {
				// process read commands
				int start_ptr = 0;
				if(buf_ptr == 0) {
					for(; start_ptr<n; start_ptr++){
						if(fifo_msg.data[start_ptr] == '$')
							break;
					}
				}
				if(buf_ptr + n > GPS_NMEA_BUF_SIZE)
					buf_ptr = 0;
				else {
					memcpy(&(buf[buf_ptr]),&(fifo_msg.data[start_ptr]),n-start_ptr);
					buf_ptr += n-start_ptr;

					int size = 0;
					bool full_packet = false; 
					for(; size < buf_ptr; size++) {
						if(buf[size] == '\n') {
							buf[size] = 0;
							full_packet = true;
							break;
						}
					}

					if(full_packet) {
						int pkt_type = processNMEA(gps, (char *)buf); 
						if( pkt_type == GPGGA_CMD ) transmit_data = true;

						if(buf_ptr-(size+1) > 0) { 
							memcpy(buf,&(buf[size+1]),buf_ptr-(size+1));
							buf_ptr = buf_ptr-(size+1);
						}
						else 
							buf_ptr = 0;
					}
				}
			}
		}
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			//cout << "fifo_FDs[INBOARD_TX]" << endl;
			if(transmit_data) {
				Command cmd;
				GPSTelem_t * dataPtr = (GPSTelem_t *)CMD_DATAPTR(fifo_msg.data);

				int hh, mm; float ss;
				float tmp = atof(gps.utc);
				hh = (int)floor( tmp / 10000.0 ); 
				mm = (int)floor( (tmp - hh*10000) / 100.0 ); 
				ss = (float)( (tmp - hh*10000 - mm*100) ); 

				CMD_CMD((fifo_msg.data)) = CMD_GPS_TELEM;
				CMD_NUM((fifo_msg.data)) = sizeof(GPSTelem_t);
				dataPtr->utc.hours          = hh; 
				dataPtr->utc.minutes        = mm; 
				dataPtr->utc.seconds        = ss; 
				dataPtr->pos.lat            = gps.lat;
				dataPtr->pos.lon            = gps.lon;
				dataPtr->pos.alt            = gps.alt;
				dataPtr->track              = gps.track;
				dataPtr->sog                = gps.speed;
				dataPtr->accuracy.sats      = gps.sv;
				dataPtr->accuracy.quality   = gps.quality;
				dataPtr->accuracy.pdop      = gps.pdop;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);
				fifo_msg.port = port;

				Client * a_client;
				Client * me = shmPtr->clients.getClient(0);
				int i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_GPS_TELEM,i)) != NULL)
				{
					//cout << "Sending to subscribed client " << a_client->name << endl;
					fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
					i++;
				}
				//if( i > 0 )
					//cout << "sent gps data to " << i << " clients " << endl;

				transmit_data = false;
			}
		}
	}
}
